#include "CStructure.h"

CStructure::CStructure()
{
    nBeams=0;
    nNodes=0;
    nUnknownForces=0;
    nActiveUnknownForces=0;
}

CStructure::~CStructure()
{
    //dtor
}

void CStructure::addNode(CNode *n)
{
    nNodes++;
    nodes[nNodes-1]=n;
}

void CStructure::addBeam(CBeam *b)
{
    int i;
    nBeams++;
    beams[nBeams-1]=b;
    CNode *firstNode,*secondNode;
    bool found=0;
    if (b->getNode(1)->getId()>b->getNode(0)->getId())
    {
        firstNode=b->getNode(0);
        secondNode=b->getNode(1);
    }
    else
    {
        firstNode=b->getNode(1);
        secondNode=b->getNode(0);
    }
    for (i=0;i<nNodes;i++)
        if (nodes[i]==firstNode)
            found=1;
    if (found==0)
    {
        nNodes++;
        nodes[nNodes-1]=firstNode;
    }
    found=0;
    for (i=0;i<nNodes;i++)
        if (nodes[i]==secondNode)
            found=1;
    if (found==0)
    {
        nNodes++;
        nodes[nNodes-1]=secondNode;
    }
}

void CStructure::assemble()
{
    FILE *test;
    test=fopen("test.txt","w");
    int i,j,k;
    if (K!=NULL)
       delete K;
    K=new CMatrix(nNodes*6,nNodes*6);
    if (rhs!=NULL)
       delete rhs;
    rhs=new CMatrix(nNodes*6,1);
    for (i=0;i<nBeams;i++)
    {
        beams[i]->createStiffnessMatrix();
        beams[i]->getStiffnessMatrix()->print();
        int n0=beams[i]->getNode(0)->getId();
        int n1=beams[i]->getNode(1)->getId();
        fprintf(test,"beam %d nodes %d %d\n",i,n0,n1);
        for (j=0;j<12;j++)
            for (k=0;k<12;k++)
            {
                int ndof_r, ndof_c;
                if (j<6)
                    ndof_r=n0*6+j;
                else
                    ndof_r=n1*6+(j-6);
                if (k<6)
                    ndof_c=n0*6+k;
                else
                    ndof_c=n1*6+(k-6);
                double prev=K->get(ndof_r,ndof_c);
                K->set(ndof_r,ndof_c,prev+beams[i]->getStiffnessMatrix()->get(j,k));
                if (i==nBeams-1)
                {
                    if (beams[i]->getStiffnessMatrix()->get(j,k)!=0.)
                        fprintf(test,"j %d k %d ndof_r %d ndof_c %d value %f\n",j, k, ndof_r, ndof_c,beams[i]->getStiffnessMatrix()->get(j,k));
                }
            }
    }
    //K->print();

    for (i=0;i<nNodes;i++)
    {
        for (j=0;j<6;j++)
        {
            if (nodes[i]->checkDisplacement(j))
            {
                int nn=nodes[i]->getId();
                int ndof=nn*6+j;
                double prev=K->get(ndof,ndof);
                K->set(ndof,ndof,prev*PENALTY);
                double value=nodes[i]->getBCValue(j);
                rhs->set(ndof,0,prev*PENALTY*value);
            }
            if (nodes[i]->checkForce(j))
            {
                int nn=nodes[i]->getId();
                int ndof=nn*6+j;
                double prev=rhs->get(ndof,0);
                double value=nodes[i]->getBCValue(j);
                rhs->set(ndof,0,prev+value);
            }
        }
    }
    //K->print();
    fclose(test);
}

void CStructure::solve()
{
    if (solution!=NULL)
       delete solution;
    solution=K->solveGauss(rhs);
    //solution=K->solveGaussSeidel(rhs);
    solution->print();
}

void CStructure::createUnknownMatrix()
{
    int i,j;
    for (i=0;i<nNodes;i++)
    {
        if (nodes[i]->checkUnknown())
        {
            nUnknownForces++;
            unknownForces[nodes[i]->getId()]=nUnknownForces-1;
        }
    }
    if (unknownMatrix!=NULL)
       delete unknownMatrix;
    unknownMatrix=new CMatrix(nNodes*6,nUnknownForces*3);
    for (i=0;i<nNodes;i++)
    {
        if (nodes[i]->checkUnknown())
        {
            int nn=nodes[i]->getId();
            int pos=unknownForces[nn]*3;
            unknownMatrix->set(nn*6,pos,nodes[i]->getUnknownDirection(X));
            unknownMatrix->set(nn*6+1,pos+1,nodes[i]->getUnknownDirection(Y));
            unknownMatrix->set(nn*6+2,pos+2,nodes[i]->getUnknownDirection(Z));
        }
    }
    unknownMatrix->print();
}

void CStructure::solveOptimized()
{
    int i,j;
    assemble();
    createUnknownMatrix();
    FILE *opfFile;
    opfFile=fopen("input.opf","w");
    fprintf(opfFile,"[objective minimize 'obj']\nob");
    fprintf(opfFile,"\n[/objective]\n\n");
    fprintf(opfFile,"[constraints]\n");
    for (i=0;i<K->getNRows();i++)
    {
        fprintf(opfFile,"[con 'c%d']",i+1);
        for (j=0;j<K->getNCols();j++)
        {
            if (K->get(i,j)>0.)
            {
                fprintf(opfFile,"+%f x%d ",K->get(i,j),j+1);
            }
            else if (K->get(i,j)<0.)
            {
                fprintf(opfFile,"%f x%d ",K->get(i,j),j+1);
            }
        }
        for (j=0;j<nUnknownForces*3;j++)
        {
            if (unknownMatrix->get(i,j)>0.)
                fprintf(opfFile,"%f x%d ",-unknownMatrix->get(i,j),K->getNCols()+((int) (j/3)) +1);
            if (unknownMatrix->get(i,j)<0.)
                fprintf(opfFile,"+%f x%d ",-unknownMatrix->get(i,j),K->getNCols()+((int) (j/3))+1);
        }
        fprintf(opfFile," = %f [/con]\n",rhs->get(i,0));
    }

    fprintf(opfFile,"[con 'c%d']",K->getNRows()+1);
    for (i=0;i<nUnknownForces;i++)
        fprintf(opfFile,"+x%d ",nNodes*6+i+1);
    fprintf(opfFile," -ob <= 0 [/con]\n");
    fprintf(opfFile,"[con 'c%d']",K->getNRows()+2);
    for (i=0;i<nUnknownForces;i++)
        fprintf(opfFile,"-x%d ",nNodes*6+i+1);
    fprintf(opfFile," -ob <= 0 [/con]\n");

    int nconst=K->getNRows()+2;
    for (i=0;i<nNodes;i++)
    {
        if (nodes[i]->checkBilateralUnknown())
        {
            nconst++;
            fprintf(opfFile,"[con 'c%d'] x%d -x%d = 0 [/con]\n",nconst,unknownForces[nodes[i]->getId()]+nNodes*6+1,unknownForces[nodes[i]->getBilateralUnknownNode()->getId()]+nNodes*6+1);
        }
    }

    fprintf(opfFile,"[/constraints]\n\n");
    fprintf(opfFile,"[bounds]\n");
    for (i=0;i<nNodes;i++)
    {
        int nn=nodes[i]->getId();
        if (nodes[i]->getRestriction(X))
            fprintf(opfFile,"[b] %f <= x%d <= %f [/b]\n",-TOLERANCE_RESTRICTION_DISPLACEMENT,nn*6+1,TOLERANCE_RESTRICTION_DISPLACEMENT);
        if (nodes[i]->getRestriction(Y))
            fprintf(opfFile,"[b] %f <= x%d <= %f [/b]\n",-TOLERANCE_RESTRICTION_DISPLACEMENT,nn*6+2,TOLERANCE_RESTRICTION_DISPLACEMENT);
        if (nodes[i]->getRestriction(Z))
            fprintf(opfFile,"[b] %f <= x%d <= %f [/b]\n",-TOLERANCE_RESTRICTION_DISPLACEMENT,nn*6+3,TOLERANCE_RESTRICTION_DISPLACEMENT);
        if (nodes[i]->getRestriction(ROTX))
            fprintf(opfFile,"[b] %f <= x%d <= %f [/b]\n",-TOLERANCE_RESTRICTION_ROTATION,nn*6+4,TOLERANCE_RESTRICTION_ROTATION);
        if (nodes[i]->getRestriction(ROTY))
            fprintf(opfFile,"[b] %f <= x%d <= %f [/b]\n",-TOLERANCE_RESTRICTION_ROTATION,nn*6+5,TOLERANCE_RESTRICTION_ROTATION);
        if (nodes[i]->getRestriction(ROTZ))
            fprintf(opfFile,"[b] %f <= x%d <= %f [/b]\n",-TOLERANCE_RESTRICTION_ROTATION,nn*6+6,TOLERANCE_RESTRICTION_ROTATION);
    }
    for (i=0;i<nUnknownForces;i++)
        fprintf(opfFile,"[b] x%d >= 0 [/b]\n",nNodes*6+i+1);
    fprintf(opfFile,"[/bounds]\n\n");
    fclose(opfFile);
    system("mosek -p mosek.par input.opf");
    //system("mosek input.opf");
    parseMosekSolution();
}

void CStructure::parseMosekSolution()
{
    int i,j;
    FILE *solFile;
    solFile=fopen("input.bas","r");
    while (!feof(solFile))
    {
        char line[255];
        fscanf(solFile,"%s",&line);
        if ((strcmp(line,"VARIABLES"))==0)
        {
            for (i=0;i<13;i++)
            {
                if (!feof(solFile))
                    fscanf(solFile,"%s",&line);
            }
            while (!feof(solFile))
            {
                fscanf(solFile,"%s",&line);
                int index;
                sscanf(line,"x%d",&index);
                //printf("variable x%d ",index);
                fscanf(solFile,"%s",&line);
                fscanf(solFile,"%s",&line);
                float value;
                sscanf(line,"%f",&value);
                //printf("value %f\n",value);
                for (i=0;i<5;i++)
                {
                    if (!feof(solFile))
                        fscanf(solFile,"%s",&line);
                }
                index--;
                if (index<(nNodes*6))
                {
                    for (i=0;i<nNodes;i++)
                    {
                        int nn=nodes[i]->getId();
                        if (floor(index/6)==nn)
                        {
                            int dir=index%6;
                            nodes[i]->setSolution(dir,(double) value);
                            //printf("solution index %d node %d dir %d value %f\n",index,i,dir,value);
                        }
                    }
                }
                else
                {
                    //printf("index %d ",index);
                    for (j=0;j<nNodes;j++)
                    {
                        if (nNodes*6+unknownForces[nodes[j]->getId()]==index)
                        {
                            //printf(" unk %d node %d value %f\n",i, nodes[j]->getId(), value);
                            int nn=nodes[j]->getId();
                            int pos=unknownForces[nn]*3;
                            double valueX=unknownMatrix->get(nn*6,pos)*value;
                            double valueY=unknownMatrix->get(nn*6+1,pos+1)*value;
                            double valueZ=unknownMatrix->get(nn*6+2,pos+2)*value;
                            nodes[j]->setUnknownSolution(X,valueX);
                            nodes[j]->setUnknownSolution(Y,valueY);
                            nodes[j]->setUnknownSolution(Z,valueZ);
                        }

                    }
                }
            }
        }
    }
    fclose(solFile);
    for (i=0;i<nNodes;i++)
    {
        //printf("node %d solution %f %f %f\n",nodes[i]->getId(),nodes[i]->getSolution(X),nodes[i]->getSolution(Y),nodes[i]->getSolution(ROT));
        if (nodes[i]->checkUnknown())
            printf("    unknown %f %f %f\n",nodes[i]->getUnknownSolution(X),nodes[i]->getUnknownSolution(Y),nodes[i]->getUnknownSolution(Z));
    }
}

void CStructure::writeAnsysFile(char filename[255])
{
    FILE *ansysFile;
    int i;
    ansysFile=fopen(filename,"w");
    fprintf(ansysFile,"/PREP7\n");
    fprintf(ansysFile,"ET,1,BEAM4\n");
    for (i=0;i<nNodes;i++)
    {
        fprintf(ansysFile,"N,%d,%f,%f,%f\n",i+1,nodes[i]->getCoord(X),nodes[i]->getCoord(Y),nodes[i]->getCoord(Z));
    }
    for (i=0;i<nBeams;i++)
    {
        fprintf(ansysFile,"E,%d,%d\n",beams[i]->getNode(0)->getId()+1,beams[i]->getNode(1)->getId()+1);
    }
    fprintf(ansysFile,"CM,UNDEFORMED,ELEM\nESEL,NONE\n");
    for (i=0;i<nNodes;i++)
    {
        fprintf(ansysFile,"N,%d,%f,%f,%f\n",i+10001,nodes[i]->getCoord(X)+nodes[i]->getSolution(X),nodes[i]->getCoord(Y)+nodes[i]->getSolution(Y),nodes[i]->getCoord(Z)+nodes[i]->getSolution(Z));
    }
    for (i=0;i<nBeams;i++)
    {
        fprintf(ansysFile,"E,%d,%d\n",beams[i]->getNode(0)->getId()+10001,beams[i]->getNode(1)->getId()+10001);
    }
    fprintf(ansysFile,"CM,DEFORMED,ELEM\nALLSEL,ALL");
    fclose(ansysFile);
}

void CStructure::parseInputFile(char filename[255])
{
    FILE *inputFile;
    int i, j, prop, n1, n2;
    float x,y,z,a,e,jy,gj;
    int properties[MAX_BEAMS];
    inputFile=fopen(filename,"r");
    while (!feof(inputFile))
    {
        char line[255];
        fscanf(inputFile,"%s",&line);
        if (strcmp(line,"NODES")==0)
        {
            bool exit=0;
            int nnode=-1;
            while (exit==0)
            {
                fscanf(inputFile,"%s",&line);
                if (strcmp(line,"ENDNODES")==0)
                    exit=1;
                else
                {
                    nnode++;
                    float x,y,z;
                    int length=strlen(line);
                    fseek(inputFile,-length,SEEK_CUR);
                    fscanf(inputFile,"%f %f %f",&x, &y,&z);
                    CNode *n=new CNode(nnode,(double) x,(double) y,(double) z);
                    printf("node %d %f %f %f\n",nnode, x, y, z);
                    addNode(n);
                }
            }
        }
    }
    rewind(inputFile);
    while (!feof(inputFile))
    {
        char line[255];
        fscanf(inputFile,"%s",&line);
        if (strcmp(line,"BEAMS")==0)
        {
            bool exit=0;
            int nbeam=-1;
            while (exit==0)
            {
                fscanf(inputFile,"%s",&line);
                if (strcmp(line,"ENDBEAMS")==0)
                    exit=1;
                else
                {
                    nbeam++;
                    int n1,n2;
                    float value;
                    int length=strlen(line);
                    fseek(inputFile,-length,SEEK_CUR);
                    fscanf(inputFile,"%d %d %d",&n1, &n2, &prop);
                    CBeam *b=new CBeam(nodes[n1],nodes[n2]);
                    addBeam(b);
                    printf("beam %d %d %d\n",nbeam,n1, n2);
                    properties[nBeams-1]=prop;
                }
            }
        }
    }

    rewind(inputFile);
    while (!feof(inputFile))
    {
        char line[255];
        fscanf(inputFile,"%s",&line);
        if (strcmp(line,"PROPERTIES")==0)
        {
            bool exit=0;
            int nprop=-1;
            while (exit==0)
            {
                fscanf(inputFile,"%s",&line);
                if (strcmp(line,"ENDPROPERTIES")==0)
                    exit=1;
                else
                {
                    nprop++;
                    int n,dir;
                    float value;
                    int length=strlen(line);
                    fseek(inputFile,-length,SEEK_CUR);
                    fscanf(inputFile,"%f %f %f %f",&a, &e, &jy, &gj);
                    printf("property %d %f %f %f %f\n",nprop,(double) a,(double) e,(double) jy,(double) gj);
                    for (j=0;j<nBeams;j++)
                    {
                        if (properties[j]==nprop)
                        {
                            beams[j]->setProperties(a,e,jy,jy,gj);
                            printf("    set to beam %d\n",j);
                        }
                    }
                }
            }
        }
    }
    rewind(inputFile);
    while (!feof(inputFile))
    {
        char line[255];
        fscanf(inputFile,"%s",&line);
        if (strcmp(line,"BCS")==0)
        {
            bool exit=0;
            while (exit==0)
            {
                fscanf(inputFile,"%s",&line);
                if (strcmp(line,"ENDBCS")==0)
                    exit=1;
                else
                {
                    int n,dir;
                    float value;
                    int length=strlen(line);
                    fseek(inputFile,-length,SEEK_CUR);
                    fscanf(inputFile,"%d %d %f",&n, &dir, &value);
                    nodes[n]->setDisplacement(dir,(double) value);
                    printf("set BC %d dir %d value %f\n",n,dir,value);
                }
            }
        }
    }
    rewind(inputFile);
    while (!feof(inputFile))
    {
        char line[255];
        fscanf(inputFile,"%s",&line);
        if (strcmp(line,"FORCES")==0)
        {
            bool exit=0;
            while (exit==0)
            {
                fscanf(inputFile,"%s",&line);
                if (strcmp(line,"ENDFORCES")==0)
                    exit=1;
                else
                {
                    int n,dir;
                    float value;
                    int length=strlen(line);
                    fseek(inputFile,-length,SEEK_CUR);
                    fscanf(inputFile,"%d %d %f",&n, &dir, &value);
                    nodes[n]->setForce(dir,(double) value);
                    printf("set force %d dir %d value %f\n",n,dir,value);
                }
            }
        }
    }
    rewind(inputFile);
    while (!feof(inputFile))
    {
        char line[255];
        fscanf(inputFile,"%s",&line);
        if (strcmp(line,"UNKNOWNFORCES")==0)
        {
            bool exit=0;
            while (exit==0)
            {
                fscanf(inputFile,"%s",&line);
                if (strcmp(line,"ENDUNKNOWNFORCES")==0)
                    exit=1;
                else
                {
                    int n1,n2;
                    float value;
                    int length=strlen(line);
                    fseek(inputFile,-length,SEEK_CUR);
                    fscanf(inputFile,"%d %d",&n1, &n2);
                    nodes[n1]->setBilateralUnknownForce(nodes[n2]);
                    printf("set unknown force %d %d\n",n1,n2);
                }
            }
        }
    }
    rewind(inputFile);
    while (!feof(inputFile))
    {
        char line[255];
        fscanf(inputFile,"%s",&line);
        if (strcmp(line,"RESTRICTIONS")==0)
        {
            bool exit=0;
            while (exit==0)
            {
                fscanf(inputFile,"%s",&line);
                if (strcmp(line,"ENDRESTRICTIONS")==0)
                    exit=1;
                else
                {
                    int n,dir;
                    float value;
                    int length=strlen(line);
                    fseek(inputFile,-length,SEEK_CUR);
                    fscanf(inputFile,"%d %d",&n, &dir);
                    nodes[n]->setRestriction(dir);
                    printf("set restriction %d dir %d\n",n,dir);
                }
            }
        }
    }
    fclose(inputFile);
}
